package com.gitee.wsl.mathematics.matrix.mat4.ext

import com.gitee.wsl.mathematics.matrix.mat4.Matrix4
import com.gitee.wsl.mathematics.number.compareTo
import com.gitee.wsl.mathematics.vector.vec3.MutableVector3
import com.gitee.wsl.unit.angle.AngleUnit
import com.gitee.wsl.unit.angle.toRadians
import kotlin.math.PI


/** @return angle */
fun<T:Number> Matrix4<T, *, *>.axisAngle(axis: MutableVector3<T, *>): AngleUnit {
    var angle = zero
    val epsilon = 0.01f
    val epsilon2 = 0.1f

    if (abs(this[1, 0] - this[0, 1]) < epsilon && abs(this[2, 0] - this[0, 2]) < epsilon && abs(this[2, 1] - this[1, 2]) < epsilon) {
        if (abs(this[1, 0] + this[0, 1]) < epsilon2 && abs(this[2, 0] + this[0, 2]) < epsilon2 && abs(this[2, 1] + this[1, 2]) < epsilon2 && abs(this[0, 0] + this[1, 1] + this[2, 2] - 3f) < epsilon2) {
            axis.x = one
            axis.y = zero
            axis.z = zero
            return angle.toRadians()
        }
        angle = PI + zero
        val xx = (this[0, 0] + 1f) * 0.5f
        val yy = (this[1, 1] + 1f) * 0.5f
        val zz = (this[2, 2] + 1f) * 0.5f
        val xy = (this[1, 0] + this[0, 1]) * 0.25f
        val xz = (this[2, 0] + this[0, 2]) * 0.25f
        val yz = (this[2, 1] + this[1, 2]) * 0.25f
        when {
            xx > yy && xx > zz -> when {
                xx < epsilon -> {
                    axis.x = zero
                    axis.y = 0.7071f+zero
                    axis.z = 0.7071f+zero
                }
                else -> {
                    axis.x = sqrt(xx)
                    axis.y = xy / axis.x
                    axis.z = xz / axis.x
                }
            }
            yy > zz -> when {
                yy < epsilon -> {
                    axis.x = 0.7071f+zero
                    axis.y = zero
                    axis.z = 0.7071f+zero
                }
                else -> {
                    axis.y = sqrt(yy)
                    axis.x = xy / axis.y
                    axis.z = yz / axis.y
                }
            }
            else -> when {
                zz < epsilon -> {
                    axis.x = 0.7071f +zero
                    axis.y = 0.7071f +zero
                    axis.z = zero
                }
                else -> {
                    axis.z = sqrt(zz)
                    axis.x = xz / axis.z
                    axis.y = yz / axis.z
                }
            }
        }
        return angle.toRadians()
    }
    var s = sqrt((this[2, 1] - this[1, 2]) * (this[2, 1] - this[1, 2]) + (this[2, 0] - this[0, 2]) * (this[2, 0] - this[0, 2]) + (this[1, 0] - this[0, 1]) * (this[1, 0] - this[0, 1]))
    if (abs(s) < 0.001f)
        s = one
    val angleCos = (this[0, 0] + this[1, 1] + this[2, 2] - 1f) * 0.5f
    angle = when {
        angleCos - 1f < epsilon -> PI * 0.25f + zero
        else -> acos(angleCos)
    }
    axis.x = (this[1][2] - this[2][1]) / s
    axis.y = (this[2][0] - this[0][2]) / s
    axis.z = (this[0][1] - this[1][0]) / s

    return angle.toRadians()
}
